#ifndef __MAHONY_H__
#define __MAHONY_H__

#include "main.h"

typedef struct
{
	float n;
	float cnt;
	float sum;
	float average;
	float flag;
}Mahony_Filiter_TypeDef;

typedef struct
{
	float last_0;
	float last_1;
	float last_2;
	float last_3;
	float last_4;
	float last_5;  
	float last_6;  
}Mahony_Last_TypeDef;

typedef struct 
{
	int n;
	Mahony_Last_TypeDef Last[6]; 
	Mahony_Filiter_TypeDef Mahony_Filiter[3];
    float delta_time;
    float gx,gy,gz;
	float ax,ay,az;
	float last_ez;
    float q0,q1,q2,q3;
    float exInt,eyInt,ezInt;
    float KP1,KP2,KI1,KI2,KD;
	float pitch,roll,yaw;
}Mahony_TypeDef;

extern Mahony_TypeDef Mahony_Data;

void Mahony_init(Mahony_TypeDef* m,int n,float time,float KP1, float KP2,float KI1,float KI2,float KD);
void Mahony_update(Mahony_TypeDef* m,float gx, float gy, float gz, float ax, float ay, float az);
void Mahony_cal(Mahony_TypeDef* m);
void Mahony_arm_math_cal(Mahony_TypeDef* m);
void Mahony_exe();

#endif
